#!/usr/bin/env python3
"""
Smart Voice Processor - 智能语音处理器
增强语音理解和命令解析能力
"""

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
from std_msgs.msg import String
from geometry_msgs.msg import Twist
import re
import json
from typing import Dict, List, Optional, Tuple
import time

class SmartVoiceProcessor(Node):
    def __init__(self):
        super().__init__('smart_voice_processor')
        
        # 增强的命令词典
        self.command_patterns = {
            # 基础移动命令
            'forward': {
                'patterns': [r'前进', r'向前', r'往前', r'前面', r'go forward', r'move forward'],
                'action': {'linear': 0.3, 'angular': 0.0}
            },
            'backward': {
                'patterns': [r'后退', r'向后', r'往后', r'倒退', r'go back', r'move back'],
                'action': {'linear': -0.3, 'angular': 0.0}
            },
            'left': {
                'patterns': [r'左转', r'向左', r'往左', r'左边', r'turn left'],
                'action': {'linear': 0.0, 'angular': 1.0}
            },
            'right': {
                'patterns': [r'右转', r'向右', r'往右', r'右边', r'turn right'],
                'action': {'linear': 0.0, 'angular': -1.0}
            },
            'stop': {
                'patterns': [r'停止', r'停下', r'暂停', r'stop', r'halt', r'brake'],
                'action': {'linear': 0.0, 'angular': 0.0}
            },
            
            # 速度修饰词
            'fast_forward': {
                'patterns': [r'快速前进', r'快点前进', r'加速前进', r'fast forward'],
                'action': {'linear': 0.5, 'angular': 0.0}
            },
            'slow_forward': {
                'patterns': [r'慢速前进', r'慢点前进', r'缓慢前进', r'slow forward'],
                'action': {'linear': 0.15, 'angular': 0.0}
            },
            
            # 复合动作
            'circle_left': {
                'patterns': [r'左转圈', r'向左转圈', r'左边转圈'],
                'action': {'linear': 0.1, 'angular': 1.5}
            },
            'circle_right': {
                'patterns': [r'右转圈', r'向右转圈', r'右边转圈'],
                'action': {'linear': 0.1, 'angular': -1.5}
            },
            
            # 距离和时间修饰
            'short_forward': {
                'patterns': [r'前进一点', r'稍微前进', r'前进一下'],
                'action': {'linear': 0.2, 'angular': 0.0, 'duration': 1.0}
            },
            'long_forward': {
                'patterns': [r'前进很久', r'一直前进', r'持续前进'],
                'action': {'linear': 0.3, 'angular': 0.0, 'duration': 5.0}
            }
        }
        
        # 数字识别
        self.number_patterns = {
            '一': 1, '二': 2, '三': 3, '四': 4, '五': 5,
            '六': 6, '七': 7, '八': 8, '九': 9, '十': 10,
            'one': 1, 'two': 2, 'three': 3, 'four': 4, 'five': 5
        }
        
        # 状态跟踪
        self.last_command = None
        self.command_history = []
        self.confidence_threshold = 0.7
        
        # 设置QoS配置
        self._setup_qos_profiles()
        
        # 发布器和订阅器 - 发布到标准的cmd_vel话题供micro-ros-agent使用
        self.cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel', self.cmd_vel_qos)
        self.voice_sub = self.create_subscription(
            String, 'voice_text', self.process_voice_text, 10)
        
        # 状态发布器
        self.status_pub = self.create_publisher(String, 'voice_status', self.status_qos)
        
        self.get_logger().info("Smart Voice Processor initialized")

    def _setup_qos_profiles(self):
        """Setup QoS profiles for different types of communication."""
        # Best effort QoS for cmd_vel (real-time control, allows packet loss)
        self.cmd_vel_qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE,
            depth=1
        )
        
        # Reliable QoS for status messages (important information)
        self.status_qos = QoSProfile(
            reliability=ReliabilityPolicy.RELIABLE,
            durability=DurabilityPolicy.VOLATILE,
            depth=10
        )
        
        self.get_logger().info("QoS profiles configured: cmd_vel=BEST_EFFORT, status=RELIABLE")

    def process_voice_text(self, msg: String):
        """处理语音文本"""
        text = msg.data.strip().lower()
        self.get_logger().info(f"Processing voice text: {text}")
        
        # 解析命令
        command, confidence = self.parse_command(text)
        
        if command and confidence > self.confidence_threshold:
            self.execute_command(command)
            self.publish_status(f"执行命令: {command['name']}, 置信度: {confidence:.2f}")
        else:
            self.publish_status(f"未识别的命令: {text}")
            self.get_logger().warn(f"Command not recognized: {text}")

    def parse_command(self, text: str) -> Tuple[Optional[Dict], float]:
        """解析语音命令"""
        best_match = None
        best_confidence = 0.0
        
        # 预处理文本
        text = self.preprocess_text(text)
        
        # 遍历所有命令模式
        for cmd_name, cmd_info in self.command_patterns.items():
            for pattern in cmd_info['patterns']:
                if re.search(pattern, text, re.IGNORECASE):
                    confidence = self.calculate_confidence(text, pattern)
                    if confidence > best_confidence:
                        best_confidence = confidence
                        best_match = {
                            'name': cmd_name,
                            'action': cmd_info['action'].copy(),
                            'original_text': text
                        }
        
        # 检查数字修饰符
        if best_match:
            best_match = self.apply_number_modifiers(best_match, text)
        
        return best_match, best_confidence

    def preprocess_text(self, text: str) -> str:
        """预处理文本"""
        # 移除标点符号
        text = re.sub(r'[，。！？、]', '', text)
        # 统一空格
        text = re.sub(r'\s+', ' ', text)
        return text.strip()

    def calculate_confidence(self, text: str, pattern: str) -> float:
        """计算匹配置信度"""
        # 简单的置信度计算
        if pattern in text:
            return min(1.0, len(pattern) / len(text) + 0.5)
        return 0.5

    def apply_number_modifiers(self, command: Dict, text: str) -> Dict:
        """应用数字修饰符"""
        # 查找数字
        for num_word, num_value in self.number_patterns.items():
            if num_word in text:
                # 根据数字调整速度或持续时间
                if 'linear' in command['action']:
                    command['action']['linear'] *= min(2.0, num_value / 3.0)
                if 'angular' in command['action']:
                    command['action']['angular'] *= min(2.0, num_value / 3.0)
                command['action']['duration'] = num_value
                break
        
        return command

    def execute_command(self, command: Dict):
        """执行命令"""
        action = command['action']
        
        # 创建Twist消息
        twist = Twist()
        twist.linear.x = action.get('linear', 0.0)
        twist.angular.z = action.get('angular', 0.0)
        
        # 发布命令
        self.cmd_vel_pub.publish(twist)
        
        # 记录命令历史
        self.command_history.append({
            'command': command,
            'timestamp': time.time()
        })
        
        # 保持最近10个命令
        if len(self.command_history) > 10:
            self.command_history.pop(0)
        
        self.last_command = command
        self.get_logger().info(f"Executed command: {command['name']}")

    def publish_status(self, status: str):
        """发布状态信息"""
        msg = String()
        msg.data = status
        self.status_pub.publish(msg)

    def get_command_suggestions(self, partial_text: str) -> List[str]:
        """获取命令建议"""
        suggestions = []
        for cmd_name, cmd_info in self.command_patterns.items():
            for pattern in cmd_info['patterns']:
                if partial_text in pattern:
                    suggestions.append(pattern)
        return suggestions[:5]  # 返回前5个建议

def main(args=None):
    rclpy.init(args=args)
    processor = SmartVoiceProcessor()
    
    try:
        rclpy.spin(processor)
    except KeyboardInterrupt:
        pass
    finally:
        processor.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()